#include <stdio.h>
#include <termio.h>
#include <unistd.h>
#include <dynamixel.h>
#include <fcntl.h>
#include <termios.h>
#include <stdlib.h>
#include "MX28AT.h"
#include "joint.h"
#include "dmath.h"
#include "kinesiology.h"

int main()
{
    /*
	int deviceIndex = 0, baudnum = 207;
	int value1[3];
	float xyz[3], angle[3], angle_spe[3];

	printf( "\n\nRead/Write example for Linux\n\n" );
	///////// Open USB2Dynamixel ////////////
	if( dxl_initialize(deviceIndex, baudnum) == 0 )
	{
		printf( "Failed to open USB2Dynamixel!\n" );
		printf( "Press Enter key to terminate...\n" );
		getchar();
		return 0;
	}
	else
		printf( "Succeed to open USB2Dynamixel!\n" );
		
	//属性初始化
	//PID初始化
	value1[0] = 46; value1[1] = 46; value1[2] = 46;
	if(1 != set_many_servo_byte(28, value1))
		return 0;
		
	value1[0] = 32; value1[1] = 32; value1[2] = 32;
	if(1 != set_many_servo_byte(27, value1))
		return 0;
	
	value1[0] = 30; value1[1] = 30; value1[2] = 30;
	if(1 != set_many_servo_byte(26, value1))
		return 0;
		
	//ENABLE
	value1[0] = 1; value1[1] = 1; value1[2] = 1;
	if(1 != set_many_servo_byte(24, value1))
		return 0;
	
	//position初始化
	xyz[0] = 0.0; xyz[1] = 0.0; xyz[2] = -320.0;
	if(1 != set_xyz(xyz[0], xyz[1], xyz[2])) 
		return 0;
	cal_xyz_radian();			 //逆运动学求解
	get_angle(angle);			 //获取计算的弧度
	//运动到目标位置
	angle_spe[0] = 5; angle_spe[1] = 5; angle_spe[2] = 5;
	move_joint_with_spe3(angle, angle_spe);
	//更新当前坐标
	value1[0] = 0.0; value1[1] = 0.0; value1[2] = -300.0;
	set_pre_position(xyz);
	
	//逆运动学控制
	kinesiology_control_demo();
	*/
	int i;
	float xyz[3], V[3];
	float angle[3], joint_angle_spe[3];
	
	xyz[0] = 0.0;
	xyz[1] = 0.0;
	xyz[2] = -395.0;
	
	V[0] = 0.0;
	V[1] = 0.0;
	V[2] = 20.0;
	
	cal_xyz_jointSpe(xyz, V);
	get_angle(angle);
	get_joint_spe(joint_angle_spe);
	
	printf("angle:(%f, %f, %f)\n", angle[0], angle[1], angle[2]);
	printf("joint_angle_spe:(%f, %f, %f)\n", joint_angle_spe[0], joint_angle_spe[1], joint_angle_spe[2]);
	
	for(i = 0; i < 3; i++) {
	    joint_angle_spe[i] = PIFromAngle(joint_angle_spe[i]);
	}
	
	printf("MX28AT_spek:(%d, %d, %d)\n", kFromRadianSpeed(joint_angle_spe[0]), kFromRadianSpeed(joint_angle_spe[1]), kFromRadianSpeed(joint_angle_spe[2]));
	
	return 1;
}

